using System;
using Sce.PlayStation.Core;

namespace Nyanimals
{
	public class CNyanimalWalkState : CNyamimalBaseState
	{
        private CollisionSphere destination;
        private CollisionSphere NextDestination;
        private CollisionSphere dblWP;
        
        private Vector3 inDirection;
        private Vector3 outDirection;
        private Vector3 projPoint;
        private float walkTime;
        private float curTime;
        bool hitDestination;
        
        public CollisionSphere Destination
        {
            get { return destination;}
            set { destination = value;}
        }
		public CNyanimalWalkState(string szName, CNyanimal nyan) : base(szName, nyan)
		{
            inDirection = Vector3.Zero;
            outDirection = Vector3.Zero;
            projPoint = Nyaminal.Position;
            
		}

		public override void Enter ()
		{
			Nyaminal.ChangeAnimation("walk");
            walkTime = CRandom.Next(100, 500) / 100.0f; // 1sec to 5sec
            curTime = 0.0f;
            hitDestination = false;
            
            if(Nyaminal.Racing)
            {
                dblWP = new CollisionSphere(Vector3.Zero, 4); 
                destination = new CollisionSphere(Vector3.Zero, 0);
                NextDestination = new CollisionSphere(Vector3.Zero, 0);
            }
            else
            {
                dblWP = null;
                destination = null;
                NextDestination = null;
            }
			// Console.WriteLine("NyanimalWalkState::Enter()");
		}

		public override void Exit ()
		{
            if(Nyaminal.Racing)
            {
                if(dblWP != null)
                    dblWP.Clear();
                if(destination != null)
                    destination.Clear();
                if(NextDestination != null)
                    NextDestination.Clear();
            }
			// Console.WriteLine("NyanimalWalkState::Exit()");
		}

		public override void Input ()
		{		
		}

		public override void Update (float deltaTime)
		{
            // update the time
            curTime += deltaTime;
            
            if(Nyaminal.Racing)
            {
                RaceUpdate(deltaTime);
            }
            else if(destination != null)
            {
                DestUpdate(deltaTime);
            }
            else
            {
                NormUpdate(deltaTime);
            }
            
            // update velocity
            float spd = Nyaminal.Speed * deltaTime;
            Nyaminal.Pivot = Nyaminal.Position + (Nyaminal.LocalMatrix.AxisZ * spd);           
		}
        
        private void RaceUpdate(float deltaTime)
        {
            // double the waypoints radius for a smoother turn
            // TODO: make this scale depending on the angle
            dblWP.Position = destination.Position;
            dblWP.Radius = destination.Radius * 5;
            
            bool inWayPointRange = CMath.PointInSphere(dblWP, Nyaminal.Position);
            if(inWayPointRange)
            {
                // the first time we are in range
                if(hitDestination == false)
                {
                    // the InDirection is the direction from now to our destination
                    inDirection = destination.Position - Nyaminal.Position;
                    inDirection = inDirection.Normalize();
                    
                    // our next destination
                    DeterminePosition(Nyaminal.DestinationWayPoint.Next, ref NextDestination);
                    
                    // our projected position
                    projPoint = new Vector3(Nyaminal.Position.X, Nyaminal.Position.Y, Nyaminal.Position.Z);
                    
                    
                    hitDestination = true;
                }
                
                // the outDirection is the direction from our current destination to the new destination
                outDirection = NextDestination.Position - Nyaminal.Position;
                outDirection = outDirection.Normalize();
                
                // move hte projected point towards the destination
                float spd = Nyaminal.Speed * deltaTime;
                projPoint += inDirection * spd;
                
                // determin the lamda from the projected point to the destination
                Vector3 fakePos = dblWP.Position + (inDirection * dblWP.Radius);
                Vector3 fakePos2 = dblWP.Position - (inDirection * dblWP.Radius);
                float fakeRadius = CMath.Squared_Distence(fakePos, fakePos2);
                float sqDist = CMath.Squared_Distence(projPoint, fakePos);
                float sqRadi = fakeRadius;//dblWP.Radius * dblWP.Radius;
                float lamda = 1.0f - (sqDist / sqRadi); // get from 0 -> 1 to 1 -> 0
                
                // cap it                
                if(lamda < 0.0f)
                {
                    lamda = 0.0f;
                }
                
                if(lamda > 1.0f)
                {
                    lamda = 1.0f;
                }
                
                
                // get the interpolated direction
                Vector3 interpDirection = CMath.Interpolate(inDirection, outDirection, lamda);
                interpDirection = interpDirection.Normalize();
                
                // "look at" with this new direction as the new Z
                Vector3 newX = Vector3.Cross(Vector3.UnitY, interpDirection);
                newX = newX.Normalize();
                Vector3 newY = Vector3.Cross(interpDirection, newX);
                
                // new matrix
                Matrix4 newMat = Nyaminal.LocalMatrix;
                newMat.AxisZ = interpDirection;
                newMat.AxisY = newY;
                newMat.AxisX = newX;
                newMat.AxisW = Vector3.Zero;
                Nyaminal.LocalMatrix = newMat;
                
            }
            else
            {
                if(hitDestination)
                {
                    // we are out of the radius so we can head directly to the new destination
                    destination.Position = NextDestination.Position;
                    destination.Radius = NextDestination.Radius;
                    inDirection = Vector3.Zero;
                    outDirection = Vector3.Zero;
                    Nyaminal.DestinationWayPoint = Nyaminal.DestinationWayPoint.Next;
                    
                    hitDestination = false;
                }
                
                LookAtDestination(destination.Position);
            }
                
                
        }
        private void DestUpdate(float deltaTime)
        {
            // if our destination is food and it becomes held, then new decision
            if(Nyaminal.GoingTowardsFood && Nyaminal.FoodToEat.Held)
            {
                Nyaminal.GoingTowardsFood = false;
                Nyaminal.FoodToEat.Claim = false;
                Nyaminal.FoodToEat = null;
                Nyaminal.NewDecision();
            }
            else
            {
                
                // if we are in range, then we can leave
                Vector3 destPos = destination.Position;
                bool atDestination = CMath.SphereToSphere(destination, Nyaminal.BoundingSphere);
                if(atDestination)
                {
                    // we can leave
                    owningMachine.ChangeState("idle");
                }
                
                // look at hte destination positiion
                LookAtDestination(destPos);
            }
        }
        private void NormUpdate(float deltaTime)
        {
            // if we have moved longer than a time, then get a new action
            if(curTime > walkTime)
            {
                Nyaminal.NewDecision();
            }
        }
        private void LookAtDestination(Vector3 destPos)
        {
            Matrix4 lookMatris = Nyaminal.WorldMatrix;
            destPos.Y = 0.0f; // TODO: handle with ground clamping when I get around to that
            
            CMath.LookAt(ref lookMatris, destPos, Vector3.UnitY);
            lookMatris.AxisW = Vector3.Zero;                
            Nyaminal.LocalMatrix = lookMatris;
        }
        
        /// <summary>
        /// Determines a random position inside the waypoint.
        /// </summary>
        /// <param name='wp'>
        /// in: Waypoint to get position inside of
        /// </param>
        /// <param name='dest'>
        /// out: Destination found
        /// </param>
        public void DeterminePosition(CWayPoint wp, ref CollisionSphere dest)
        {
            // Determin a random position inside the waypoint's radius
            float min = wp.Sphere.Radius * -100;
            float max = wp.Sphere.Radius *  100;
            Vector3 newPos = new Vector3(wp.Sphere.Position.X, wp.Sphere.Position.Y, wp.Sphere.Position.Z);
            newPos.X += (float)CRandom.Next((int)min, (int)max) / 100.0f;
            newPos.Z += (float)CRandom.Next((int)min, (int)max) / 100.0f;
            
            // assign the new destination
            dest.Position = newPos;
            dest.Radius = wp.Sphere.Radius / 2.0f;
        }
        /*public CollisionSphere DeterminePosition(CWayPoint wp)
        {
            CollisionSphere dest;
            DeterminePosition(wp, out dest);
            return dest;
        }*/
        
        
        public void RandomDirection()
        {
            Vector3 dir = new Vector3();
            dir.X = (float)CRandom.Next(-100, 100);
            dir.Y = 0;
            dir.Z = (float)CRandom.Next(-100, 100);
            
            // fake normalize
            dir = dir * 0.01f;
            
            SetDirection(dir);
        }
        public void SetDirection(Vector3 dir)
        {
            // look at the passed in direction
            Matrix4 lookAt = Matrix4.Identity;
            CMath.LookAt(ref lookAt, dir, Vector3.UnitY);
            
            Nyaminal.LocalMatrix = lookAt;            
        }
	}
}

